/* --------------------------- (C) COPYRIGHT 2021 Fortiortech ShenZhen -----------------------------
    File Name      : MotorControl.c
    Author         : Fortiortech  Appliction Team
    Version        : V1.0
    Date           : 2021-04-10
    Description    : This file contains .C file function used for Motor Control.
----------------------------------------------------------------------------------------------------
                                       All Rights Reserved
------------------------------------------------------------------------------------------------- */
#include <MyProject.h>
/* Private variables ----------------------------------------------------------------------------*/
MotStaType mcState;
MotStaM    McStaSet;
uint8 b=0;

/* -------------------------------------------------------------------------------------------------
    Function Name  : MC_Control
    Description    : 电机控制状态机
    Date           : 2021-04-10
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void MC_Control(void)
{
    switch (mcState)
    {
        case mcReady:			//进入准备
            if (mcFaultSource != FaultNoSource)   //故障	保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {                
                Motor_Ready();
                
                if (((mcCurOffset.OffsetFlag == 1) && (MotorControl.FlagONOFF == 1))/*&&(mcFocCtrl.mcDcbusFlt >_Q15(30.0 / HW_BOARD_VOLT_MAX))*/
													&&(Time.PowerOnCnt>1000))
                {
                    mcState = mcInit;
                    //mcCurOffset.OffsetFlag = 0;
									
                }
            }
            break;
            
        case mcInit:		//初始化
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                Motor_Init();		//上电时，关闭输出，对硬件电路的电流进行采集
                #if (IPMState == IPMtest)  //IPMState  =  NormalRun   不执行该语句                   
                {
                    mcState                 = mcCharge;
                    mcFocCtrl.State_Count   = Charge_Time;
                }
                #else		//IPMState  =  NormalRun	执行该语句
                {
                    mcState                 = mcCharge;
                    mcFocCtrl.State_Count   = Charge_Time;                    //6832可以不需要预充电
                }
                #endif
            }
            
            break;
            
        case mcCharge:			//初始化完成后执行该语句
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                Motor_Charge();			//预充电
                #if (IPMState == NormalRun)                         // 正常按电机状态机运行
                {
                    if (mcFocCtrl.State_Count == 0)		//充电结束
                    {
                        MOE = 0;
                        mcState = mcTailWind;
                    }
                }
                #endif
            }
            
            break;
            
        case mcTailWind:							//顺逆风
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                #if (TailWind_Mode == NoTailWind)	// 无逆风顺风判断
                {
                    mcState                   = mcAlign;	//预定位
                    mcFocCtrl.State_Count     = Align_Time;	//预定位时间
                }
                #elif (TailWind_Mode == TailWind)
                {
                    Motor_TailWind();
                }
                #endif
            }
            
            break;
            
        case mcPosiCheck:							//初始位置检测
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                #if (PosCheckEnable==0)                             //初始位置检测不使能时初始角度为预定位角度
            
                if (mcFocCtrl.State_Count == 0)
                {
                    mcFocCtrl.mcPosCheckAngle = 0;
                    mcState = mcStart;//mcAlign;
                    mcFocCtrl.State_Count = Align_Time;
                }
                
                #endif
            }
            
            break;
            
        case mcAlign:	//预定位
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                Motor_Align();	//预定位函数
                #if (AlignTestMode == 1)
                {
                    while (1);
                }
                #else
                {
                    if (mcFocCtrl.State_Count == 0)
                    {
                        mcState = mcStart;
                    }
                }
                #endif
            }
            
            break;
            
        case mcStart:		//启动
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                Motor_Open();	//启动参数配置
            }
            
            break;
            
        case mcRun:									//运行
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                if (MotorControl.TargetValue == 0)
                {
                    mcState = mcStop;
                    mcFocCtrl.State_Count = 400;
                }
            }
            
            break;
            
        case mcStop:
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
				
                MC_Stop();
            }
            
            break;
            
        case mcBrake:
            if (mcFaultSource != FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                mcState   = mcFault;
				
            }
            else
            {
                MC_Break();
            }
            
            break;
            
        case mcFault:
            if (mcFaultSource == FaultNoSource)   //保护清零后关闭输出，软件重新运行
            {
                ClrBit(DRV_CR, FOCEN);  //FOC Disable
                MOE       = 0;
                mcState   = mcReady;
            }
            else
            {
				
                FaultProcess();
				
//							ERR_LED_LOOP(mcFaultSource);
            }
            
            _nop_();
            break;
            
        default:
            break;
    }
}

